#include "../include/7Dof_Model_dyna.h"

#include "ros/ros.h"
#include "../include/serial_struct.h"
#include "../include/hardware/robot.h"

#include <math.h>
#include <iostream>
#include <thread>
#include <condition_variable>

#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>

using std::cerr;
using std::cout;
using std::endl;
using std::exception;
using std::string;
using std::vector;
using namespace std;
using namespace Eigen;

/* action 服务端声明 */
typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> ArmServer;

// moveit 轨迹接收回调
void executeTrajectory(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal, ArmServer *as_)
{
      ROS_INFO("Get Trajectory from moveit");
      // 遍历轨迹点并发送给电机
      for (const auto &point : goal->trajectory.points)
      {
            if (as_->isPreemptRequested())
            { // 处理取消请求
                  ROS_WARN("Trajectory preempted");
                  as_->setPreempted();
                  return;
            }

            // 打印当前轨迹点的关节位置（直接插入ROS_INFO）
            ROS_INFO("--- Executing Trajectory Point ---");
            ROS_INFO("Time from start: %.3f sec", point.time_from_start.toSec());
            for (size_t i = 0; i < point.positions.size(); i++)
            {
                  ROS_INFO("  Joint %s: %.4f rad",
                           goal->trajectory.joint_names[i].c_str(),
                           point.positions[i]);
            }

            // 发送到CAN总线
            for (size_t i = 0; i < point.positions.size(); i++)
            {
            }

            // 控制执行频率（可选）
            ros::Duration(0.01).sleep(); // 10ms间隔
      }

      ROS_INFO("Trajectory execution finished");
      as_->setSucceeded();
}

int main(int argc, char **argv)
{
      ros::init(argc, argv, "arm_7dof_trajectory_action");
      ros::NodeHandle n;
      ros::Rate r(200);

      // 新增action用于接受moveit传过来的轨迹
      ArmServer as_(n, "arm_7dof/arm_controller/follow_joint_trajectory", boost::bind(&executeTrajectory, _1, &as_), false);

      // 启动 Action Server
      as_.start();
      ROS_INFO("Trajectory Action Server started");

      // 主线程只处理ROS回调
      ros::AsyncSpinner spinner(1);
      spinner.start();

      ROS_INFO("main running");

      while(ros::ok){
            //ROS_INFO("loop");
            // ros::spinOnce();
            r.sleep();
      }

      return 0;
}
